//
// Created by zhanglei on 2024/1/28.
//

#ifndef QUATERNION_KINEMATICS_H
#define QUATERNION_KINEMATICS_H
#include <deque>
#include "common/gnss.h"
#include "common/imu.h"
#include "common/odom.h"
#include "common/math_utils.h"
#include "common/nav_state.h"

namespace sad {

class QuaternionKinematics {
public:
    using SO3 = Sophus::SO3<double>;                     // 旋转变量类型
    using VecT = Eigen::Matrix<double, 3, 1>;            // 向量类型
    using Vec18T = Eigen::Matrix<double, 18, 1>;         // 18维向量类型
    using Mat3T = Eigen::Matrix<double, 3, 3>;           // 3x3矩阵类型
    using MotionNoiseT = Eigen::Matrix<double, 18, 18>;  // 运动噪声类型
    using OdomNoiseT = Eigen::Matrix<double, 3, 3>;      // 里程计噪声类型
    using GnssNoiseT = Eigen::Matrix<double, 6, 6>;      // GNSS噪声类型
    using Mat18T = Eigen::Matrix<double, 18, 18>;        // 18维方差类型
    using NavStateT = NavState<double>;                  // 整体名义状态变量类型

    enum IntegralMethod{
        MEAN_VALUE = 0,
        MEDIAN_VALUE = 1,
        RUNGE_KUTTA = 2
    };

    struct Options {
        Options() = default;
        /// IMU 测量与零偏参数
        double imu_dt_ = 0.01;  // IMU测量间隔
        double sqrt_dt = std::sqrt(imu_dt_);
        // NOTE IMU噪声项都为离散时间，不需要再乘dt，可以由初始化器指定IMU噪声
        double gyro_var_ = 1.7453e-04 * sqrt_dt;       // 陀螺测量标准差
        double acce_var_ = 5.8860e-03 * sqrt_dt;       // 加计测量标准差
        double bias_gyro_var_ = 1.0000e-05;  // 陀螺零偏游走标准差
        double bias_acce_var_ = 1.0000e-04;  // 加计零偏游走标准差

        /// 里程计参数
        double odom_var_ = 100000.0;
        double odom_span_ = 0.01;        // 里程计测量间隔
        double left_wheel_radius_ = 0.623479 / 2.0;   // 轮子半径
        double right_wheel_radius_ = 0.622806 / 2.0;   // 轮子半径
        double circle_pulse_ = 4096.0;  // 编码器每圈脉冲数

        /// RTK 观测参数
        double gnss_pos_noise_ = 0.1;                   // GNSS位置噪声
        double gnss_height_noise_ = 0.1;                // GNSS高度噪声
        double gnss_ang_noise_ = 50.0 * math::kDEG2RAD;  // GNSS旋转噪声

        /// 其他配置
        bool update_bias_gyro_ = true;  // 是否更新陀螺bias
        bool update_bias_acce_ = true;  // 是否更新加计bias
    };

    QuaternionKinematics();
    bool Predict(const IMU& imu);
    bool ObserveWheelPoseSpeed(const Odom& odom, const NavStateT& last_odom_state);

    bool ObserveGps(const GNSS& gnss);
    bool ObserveSE3(const SE3& pose, double trans_noise = 0.1, double ang_noise = 1.0 * math::kDEG2RAD);
    
    bool ObservePose(const SE3& pose, double trans_noise = 0.1, double ang_noise = 1.0 * math::kDEG2RAD);
    bool ObserveQuaternion(const SE3& pose, double trans_noise = 0.1, double ang_noise = 1.0 * math::kDEG2RAD);
    bool ObserveRotation(const SE3& pose, double trans_noise = 0.1, double ang_noise = 1.0 * math::kDEG2RAD);

    Eigen::Matrix3d ComputeRotationJacbian();

    bool ObserveWheelSpeed(const Odom& odom);


    /**
     * 设置初始条件
     * @param options 噪声项配置
     * @param init_bg 初始零偏 陀螺
     * @param init_ba 初始零偏 加计
     * @param gravity 重力
     */
    void SetInitialConditions(Options options, const Vec3d& init_bg, const Vec3d& init_ba, const Vec3d& gravity,
                              const Mat3d& init_rot) {
        BuildNoise(options);
        options_ = options;
        bg_ = init_bg;
        ba_ = init_ba;
        g_ = gravity;
        cov_ = Mat18T::Identity() * 1e-4;
    }

    NavStateT GetNominalState() const { 
        Eigen::Quaterniond rotation(rot_);
        rotation.normalize();
        return NavStateT(current_time_, Sophus::SO3d(rotation.toRotationMatrix()), p_, v_, bg_, ba_); 
    }
    SE3 GetNominalSE3() const { return SE3(rot_, p_); }
    /// 设置状态X
    void SetX(const NavStated& x, const Vec3d& grav) {
        current_time_ = x.timestamp_;
        rot_ = x.R_.matrix();
        p_ = x.p_;
        v_ = x.v_;
        bg_ = x.bg_;
        ba_ = x.ba_;
        g_ = grav;
    }

    void SetCov(const Mat18T& cov) { cov_ = cov; }
    /// 获取重力
    Vec3d GetGravity() const { return g_; }

    inline Eigen::Matrix3d SkewMatrix(const Eigen::Vector3d& v) {
        Eigen::Matrix3d w;
        w <<  0.,   -v(2),  v(1),
            v(2),  0.,   -v(0),
            -v(1),  v(0),  0.;

        return w;
    }
    /// Roderigous Tranformation
    Eigen::Matrix3d Exp(const Eigen::Vector3d &axis_angle);
    Eigen::Vector3d Log(const Eigen::Matrix3d& R);


    double GetVelocity() {
        return velocity_;
    };

private:
    void BuildNoise(const Options& options) {
        double ev = options.acce_var_;
        double et = options.gyro_var_;
        double eg = options.bias_gyro_var_;
        double ea = options.bias_acce_var_;

        double ev2 = ev;  // * ev;
        double et2 = et;  // * et;
        double eg2 = eg;  // * eg;
        double ea2 = ea;  // * ea;

        // 设置过程噪声
        Q_.diagonal() << 0, 0, 0, ev2, ev2, ev2, et2, et2, et2, eg2, eg2, eg2, ea2, ea2, ea2, 0, 0, 0;

        // 设置里程计噪声
        double o2 = options_.odom_var_ * options_.odom_var_;
        odom_noise_.diagonal() << o2, o2, o2;

        // 设置GNSS状态
        double gp2 = options.gnss_pos_noise_ * options.gnss_pos_noise_;
        double gh2 = options.gnss_height_noise_ * options.gnss_height_noise_;
        double ga2 = options.gnss_ang_noise_ * options.gnss_ang_noise_;
        gnss_noise_.diagonal() << gp2, gp2, gh2, ga2, ga2, ga2;
    }

    /// 更新名义状态变量，重置error state
    void UpdateAndReset();
    void UpdateQuaternionAndReset();
    /// 对P阵进行投影，参考式(3.63)
    void ProjectCov();
    Eigen::Matrix<double, 4, 4> ComputeRightQuaternion(const Eigen::Quaterniond& quat);
    /// 成员变量
    double current_time_ = 0.0;  // 当前时间

    /// 名义状态
    VecT p_ = VecT::Zero();
    VecT v_ = VecT::Zero();

    Mat3T rot_ = Mat3T::Identity();
    VecT bg_ = VecT::Zero();
    VecT ba_ = VecT::Zero();
    VecT g_{0, 0, -9.8};

    Eigen::Quaterniond quat_ = Eigen::Quaterniond(rot_);
    /// 误差状态
    Vec18T dx_ = Vec18T::Zero();

    /// 协方差阵
    Mat18T cov_ = Mat18T::Identity();

    /// 噪声阵
    MotionNoiseT Q_ = MotionNoiseT::Zero();
    OdomNoiseT odom_noise_ = OdomNoiseT::Zero();
    GnssNoiseT gnss_noise_ = GnssNoiseT::Zero();

    /// 标志位
    bool first_gnss_ = true;  // 是否为第一个gnss数据

    /// 配置项
    Options options_;
    GNSS gnss_;

    Odom last_odom_;  
    IMU last_imu_;  

    IntegralMethod int_method_;

    Mat3T wheel_rot_ = Mat3T::Identity();
    VecT wheel_trans_ = VecT::Zero();

    bool is_use_wheel_se3_ = true;

    double velocity_ = 0.0;

    VecT last_p_ = VecT::Zero();
    Mat3T last_rot_ = Mat3T::Identity();
};

}

#endif